
#include <math.h>
#include <stdio.h>

#include <util/types.h>
#include <util/mem.h>
#include <util/math_macros.h>
#include <util/common_macros.h>

#include "alg_primitives.h"

#define __eps 10e-15
#define __tol (0.1 * __eps)

// =============================================================================

/**
 * This function prints a matrix M at console.
 * @param rows - number of rows of the matrix.
 * @param cols - number of columns of the matrix.
 * @param M - pointer to the first matrix element.
 * @param stm - matrix stride. Number of elements from one row to the next row
 * in the memory. This value corresponds to the distance (sizeof(*M) * stm) in bytes.
 */
void __mprint(int rows, int cols, real * M, int stm) {

	int r, c;

	for (r = 0; r < rows; r++) {
		for (c = 0; c < cols; c++) {
			printf("%4.4f\t", *M);
			M++;
		}
		printf("\n");
		M += (stm - cols);
	}
	printf("\n\n");
}

// =============================================================================

/**
 * This function increments a vector by a factor k.
 * @param m - number of elements.
 * @param u - pointer to the first element of vector.
 * @param incu - increment to the next element of vector.
 * @param k - the additive parameter.
 */
void __vinc( real k, int m, real * u, int incu, real * v, int incv) {
	
	do {
		
		m--;
		
		*v = *u + k;
		u += incu;
		v += incv;
		
	} while(m >= 0);
}

/**
 * This function computes a element-by-element k power of a vector.
 * @param m - number of elements.
 * @param u - pointer to the first element of vector.
 * @param incu - increment to the next element of vector.
 * @param k - the power parameter.
 */
void __vpow( real k, int m, real * u, int incu, real * v, int incv) {
	
	do {
		
		m--;
		*v = pow( *u, k);
		u += incu;
		v += incv;
		
	} while(m >= 0);
}

/**
 * This function fills the matrix A with the matrix identity.
 * @param m - dimension of A.
 * @param A - pointer to the first matrix element.
 * @param sta - stride of A.
 */
void __eye(int m, real * A, int sta) {
	int r, c;
	real * Ai;

	Ai = A;
	for (r = 0; r < m; r++) {
		for (c = 0; c < m; c++) {

			if (r == c) *Ai = 1.0;
			else *Ai = 0.0;
			Ai++;
		}
		Ai += (sta - m);
	}

}

/**
 * This function calculates the dot/inner/scalar product of the vectors u and v.
 * @param m - number of elements of each vector.
 * @param u - pointer to the first vector element.
 * @param incu - stride of u. Number of elements in memory from the first element to the next.
 * @param v - pointer to the first vector element.
 * @param incv - stride of v. Number of elements in memory from the first element to the next.
 * @return the dot product.
 */
real __vdot(int m, real * u, int incu, real * v, int incv) {

	real dot;

	dot = 0;
	while (m) {

		dot += *u * *v;

		u += incu;
		v += incv;
		m--;
	}

	return dot;
}

/**
 * This function calculate the norm of a vector.
 * @param m - number of elements of the vector.
 * @param u - pointer to the first vector element.
 * @param incu - stride of u. Number of elements in memory from the first element to the next.
 * @return 
 */
real __vnorm(int m, real * u, int incu) {
	real norm;

	norm = __vdot(m, u, incu, u, incu);

	norm = sqrt(norm);
	return norm;
}

/**
 * This function calculates the outer product of two vectors u and v. The result is stored in O.
 * @param m - number of elements of each vector.
 * @param u - pointer to the first vector element.
 * @param incu - stride of v. Number of elements in memory from the first element to the next.
 * @param v - pointer to the first vector element.
 * @param incv - stride of v. Number of elements in memory from the first element to the next.
 * @param O - pointer to the first element of the outer product matrix.
 * @param sto - stride of O. Number of elements in memory from the first row to the next.
 */
void __vouter(int m, real * u, int incu, real * v, int incv, real * O, int sto) {
	int r, c;

	real * vi;

	for (r = 0; r < m; r++) {

		vi = v;

		*O = 0.0;

		for (c = 0; c < m; c++) {

			*O += *u * *vi;

			vi += incv;
			O++;
		}

		O += (sto - m);
		u += incu;
	}

}


/**
 * C = A;
 * @param m
 * @param n
 * @param A
 * @param sta
 * @param C
 * @param stc
 */
void __mcopy( int m, int n, real * A, int sta, real * C, int stc) {
	
	int r, c;
	
	for ( r = 0; r < m; r++ ) {
		for( c = 0; c < n; c++) {
			*C = *A;
			A++;
			C++;
		}
		A += (sta-n);
		C += (stc-n);
	}
}

void __mset( int m, int n, real * A, int sta, real val) {
	
	int r, c;
	
	for ( r = 0; r < m; r++ ) {
		for( c = 0; c < n; c++) {
			*A = val;
			A++;
		}
		A += (sta-n);
	}
}

void __mscale( int m, int n, real * A, int sta, real k, real * M, int stm) {
	
	int r, c;
	
	for ( r = 0; r < m; r++ ) {
		for( c = 0; c < n; c++) {
			*M = *A * k;
			A++;
			M++;
		}
		A += (sta-n);
		M += (stm-n);
	}
}

/**
 * This function computes a element-by-element k power of a matrix.
 * @param m - number of rows.
 * @param n - number of columns.
 * @param A - pointer to the first element of Matrix.
 * @param sta - increment to the next row of Matrix A.
 * @param P - pointer to the first element of the power Matrix.
 * @param stp - increment to the next row of Matrix P.
 * @param k - the power parameter.
 */
void __mpow( int m, int n, real * A, int sta, real k, real * P, int stp) {
	
	int r, c;
	
	for ( r = 0; r < m; r++ ) {
		for( c = 0; c < n; c++) {
			*P = pow( *A, k );
			A++;
			P++;
		}
		A += (sta-n);
		P += (stp-n);
	}
}

/**
 * This function multiplies two matrices. The result is stored in M.
 * @param rows_a - number of rows of the matrix A.
 * @param cols_a - number of columns of the matrix A.
 * @param A - pointer to the first element of the matrix.
 * @param sta - stride of A. Number of elements in memory from the first row to the next.
 * @param rows_b - number of rows of the matrix B.
 * @param cols_b - number of columns of the matrix B.
 * @param B - pointer to the first element of the matrix.
 * @param stb - stride of B. Number of elements in memory from the first row to the next.
 * @param M - pointer to the first element of the product matrix.
 * @param stm - stride of M. Number of elements in memory from the first row to the next.
 */
void __mult(int rows_a, int cols_a, real * A, int sta, int rows_b, int cols_b, real * B, int stb, real * M, int stm) {

	int r, c, k;

	real * _B = B;
	real * Ai;
	real * Bi;

	for (r = 0; r < rows_a; r++) {

		B = _B;

		for (c = 0; c < cols_b; c++) {

			Ai = A;
			Bi = B;
			*M = 0;

			for (k = 0; k < rows_b; k++) {
				*M += (*Ai) * (*Bi);

				Ai++;
				Bi += stb;
			}
			M++;
			B++;
		}
		M += (stm - cols_b);
		A += sta;
	}

}

/**
 * This function calculates the Householder transformation to the vector u. The result is stored in H.
 * @param m - number of elements of vector.
 * @param u - pointer to the first element of the vector.
 * @param incu - stride of u. Number of elements in memory from the first element to the next.
 * @param H - pointer to the first element of the transformation matrix.
 * @param sth - stride of H. Number of elements in memory from the first row to the next.
 */
int __householder(int m, real * u, int incu, real * H, int sth) {

	int r, c;
	int sign;
	real norm = __vnorm(m, u, incu);

	if (*u > 0) sign = -1;
	else sign = 1;

	if (norm <= __tol) {
		__eye(m, H, sth);
		return 1;
	}

	real * a = mem_allocn(m, real);

	int i;
	for (i = 1; i < m; i++) {
		a[i] = *(u + (i * incu));
	}

	a[0] = (*u) - sign * norm;

	__vouter(m, a, 1, a, 1, H, sth);

	real k = __vdot(m, a, 1, a, 1);

	k = -2 / k;

	real * Ht = H;
	for (r = 0; r < m; r++) {
		for (c = 0; c < m; c++) {

			*Ht = k * *Ht;
			Ht++;
		}
		Ht += (sth - m);
	}

	Ht = H;
	for (r = 0; r < m; r++) {
		for (c = 0; c < m; c++) {
			if (r == c) *Ht = 1 + *Ht;
			Ht++;
		}
		Ht += (sth - m);
	}

	free(a);

	return sign;
}

/**
 * This function calculates the QR factorization of an input m by n matrix A.
 * @param m - number of rows of the matrix A.
 * @param n - number of columns of the matrix A.
 * @param A - pointer to the first element of the matrix.
 * @param sta - stride of A. Number of elements in memory from the first row to the next.
 * @param Q  - pointer to the first element of the matrix Q. Q is an orthonormal matrix.
 * @param stq - stride of Q. Number of elements in memory from the first row to the next.
 * @param R - pointer to the first element of the matrix R. R is an upper triangular matrix.
 * @param str - stride of R. Number of elements in memory from the first row to the next.
 * 
 * TODO: Ainda precisa lidar com indeterminação e com a propagação do erro.
 */
void __qr_householder(int m, int n, real * A, int sta, real * Q, int stq, real * R, int str) {

	real * An = (real *) calloc(m*n, sizeof (real));
	real * Qn = (real *) calloc(m*m, sizeof (real));
	real * Qx = (real *) calloc(m*m, sizeof (real));

	int i;
	int i_max = (m < n) ? m - 1 : n - 1;

	mem_copymr(m, n, An, n, A, sta);

	for (i = 0; i < i_max; i++) {

		__householder(m - i, &An[ i * (n + 1) ], n, &Qn[ i * (m + 1) ], m);

		__mult(m - i, m - i, &Qn[ i * (m + 1) ], m, m - i, n - i, &An[ i * (n + 1)], n, &R[ i * (str + 1)], str);
		mem_copymr(m - i, n - i, &An[ i * (n + 1)], n, &R[ i * (str + 1)], str);

		if (i == 0) {
			mem_copymr(m, m, Q, stq, Qn, m);
			mem_copymr(m, m, Qx, m, Qn, m);
		} else {
			__mult(m, m - i, &Q[i], stq, m - i, m - i, &Qn[ i * (m + 1) ], m, &Qx[ i ], m);
			mem_copymr(m, m - i, &Q[i], stq, &Qx[ i ], m);
		}

	}
	free(An);
	free(Qn);
}


#include <malloc.h> /* for array allocation */
#include <math.h>    /* for 'fabs'           */

/*  svd.c -- Singular value decomposition. Translated to 'C' from the
 *           original Algol code in "Handbook for Automatic Computation,
 *           vol. II, Linear Algebra", Springer-Verlag.
 *
 *  (C) 2000, C. Bond. All rights reserved.
 *
 *  This is almost an exact translation from the original, except that
 *  an iteration counter is added to prevent stalls. This corresponds
 *  to similar changes in other translations.
 *
 *  Returns an error code = 0, if no errors and 'k' if a failure to
 *  converge at the 'kth' singular value.
 */
int old_svd_algol(int m, int n, int withu, int withv, double eps, double tol, double *a, double *q, double *u, double *v) {
	int i, j, k, l, l1, iter, retval;
	double c, f, g, h, s, x, y, z;
	double *e;

	e = (double *) calloc(n, sizeof (double));
	retval = 0;

	/* Copy 'a' to 'u' */
	for (i = 0; i < m; i++) {
		for (j = 0; j < n; j++) {
			u[i * n + j] = a[i * n + j];
		}
	}
	/* Householder's reduction to bidiagonal form. */
	g = x = 0.0;
	for (i = 0; i < n; i++) {
		e[i] = g;
		s = 0.0;
		l = i + 1;
		for (j = i; j < m; j++) {
			s += (u[j * n + i] * u[j * n + i]);
		}
		if (s < tol) {
			g = 0.0;
		} else {
			f = u[i * n + i];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			u[i * n + i] = f - g;
			for (j = l; j < n; j++) {
				s = 0.0;
				for (k = i; k < m; k++) {
					s += (u[k * n + i] * u[k * n + j]);
				}
				f = s / h;
				for (k = i; k < m; k++) {				
					u[k * n + j] += (f * u[k * n + i]);
				}
			} /* end j */
		} /* end s */
		q[i] = g;
		s = 0.0;
		for (j = l; j < n; j++) {
			s += (u[i * n + j] * u[i * n + j]);
		}
		if (s < tol) {
			g = 0.0;
		} else {
			f = u[i * n + (i + 1) ];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			u[i * n + (i + 1) ] = f - g;
			for (j = l; j < n; j++) {
				e[j] = u[i * n + j] / h;
			}
			for (j = l; j < m; j++) {
				s = 0.0;
				for (k = l; k < n; k++) {
					s += (u[j * n + k] * u[i * n + k]);
				}
				for (k = l; k < n; k++) {
					u[j * n + k] += (s * e[k]);
				}
			} /* end j */
		} /* end s */
		y = fabs(q[i]) + fabs(e[i]);
		if (y > x)
			x = y;
	} /* end i */
	

	/* accumulation of right-hand transformations */
	if (withv) {
		for (i = n - 1; i >= 0; i--) {
			if (g != 0.0) {
				h = u[i * n + (i + 1) ] * g;
				for (j = l; j < n; j++) {
					v[j * n + i] = u[i * n + j] / h;
				}
				for (j = l; j < n; j++) {
					s = 0.0;
					for (k = l; k < n; k++) {
						s += (u[i * n + k] * v[k * n + j]);
					}
					for (k = l; k < n; k++) {
						v[k * n + j] += (s * v[k * n + i]);
					}

				} /* end j */
			} /* end g */
			for (j = l; j < n; j++) {
				v[i * n + j] = v[j * n + i] = 0.0;
			}
			v[i * n + i] = 1.0;
			g = e[i];
			l = i;
		} /* end i */

	} /* end withv, parens added for clarity */

	/* accumulation of left-hand transformations */

	if (withu) {
	//if (0) {
		for (i = n; i < m; i++) {
			for (j = n; j < m; j++) {
				u[i * n + j] = 0.0;
			}
			u[i * n + i] = 1.0;
		}
	}
	if (withu) {
	//if (0) {
		for (i = n - 1; i >= 0; i--) {
			l = i + 1;
			g = q[i];
			for (j = l; j < m; j++) { /* upper limit was 'n' */
				u[i * n + j] = 0.0;
			}
			if (g != 0.0) {
				h = u[i * n + i] * g;
				for (j = l; j < m; j++) { /* upper limit was 'n' */
					s = 0.0;
					for (k = l; k < m; k++) {
						s += (u[k * n + i] * u[k * n + j]);
					}
					f = s / h;
					for (k = i; k < m; k++) {
						u[k * n + j] += (f * u[k * n + i]);
					}
				} /* end j */
				for (j = i; j < m; j++) {
					u[j * n + i] /= g;
				}
			}/* end g */
			else {
				for (j = i; j < m; j++) {
					u[j * n + i] = 0.0;
				}
			}
			u[i * n + i] += 1.0;
		} /* end i*/
	} /* end withu, parens added for clarity */

	/* diagonalization of the bidiagonal form */
	eps *= x;
	for (k = n - 1; k >= 0; k--) {
		iter = 0;
test_f_splitting:
		for (l = k; l >= 0; l--) {
			if (fabs(e[l]) <= eps) goto test_f_convergence;
			if (fabs(q[l - 1]) <= eps) goto cancellation;
		} /* end l */

		/* cancellation of e[l] if l > 0 */
cancellation:
		c = 0.0;
		s = 1.0;
		l1 = l - 1;
		for (i = l; i <= k; i++) {
			f = s * e[i];
			e[i] *= c;
			if (fabs(f) <= eps) {
				goto test_f_convergence;
			}
			g = q[i];
			h = q[i] = sqrt(f * f + g * g);
			c = g / h;
			s = -f / h;
			if (withu) {
				//if (0) {
				for (j = 0; j < m; j++) {
					y = u[j * n + l1];
					z = u[j * n + i];
					u[j * n + l1] = y * c + z * s;
					u[j * n + i] = -y * s + z * c;
				} /* end j */
			} /* end withu, parens added for clarity */
		} /* end i */
test_f_convergence:
		z = q[k];
		if (l == k) {
			goto convergence;
		}

		/* shift from bottom 2x2 minor */
		iter++;
		if (iter > 30) {
			retval = k;
			break;
		}
		x = q[l];
		y = q[k - 1];
		g = e[k - 1];
		h = e[k];
		f = ((y - z)*(y + z) + (g - h)*(g + h)) / (2 * h * y);
		g = sqrt(f * f + 1.0);
		f = ((x - z)*(x + z) + h * (y / ((f < 0) ? (f - g) : (f + g)) - h)) / x;
		/* next QR transformation */
		c = s = 1.0;
		for (i = l + 1; i <= k; i++) {
			g = e[i];
			y = q[i];
			h = s * g;
			g *= c;
			e[i - 1] = z = sqrt(f * f + h * h);
			c = f / z;
			s = h / z;
			f = x * c + g * s;
			g = -x * s + g * c;
			h = y * s;
			y *= c;
			if (withv) {
				for (j = 0; j < n; j++) {
					x = v[j * n + (i - 1)];
					z = v[j * n + i];
					v[j * n + (i - 1)] = x * c + z * s;
					v[j * n + i] = -x * s + z * c;
				} /* end j */
			} /* end withv, parens added for clarity */
			q[i - 1] = z = sqrt(f * f + h * h);
			c = f / z;
			s = h / z;
			f = c * g + s * y;
			x = -s * g + c * y;
			if (withu) {
				//if (0) {
				for (j = 0; j < m; j++) {
					y = u[j * n + (i - 1)];
					z = u[j * n + i];
					u[j * n + (i - 1)] = y * c + z * s;
					u[j * n + i] = -y * s + z * c;
				} /* end j */
			} /* end withu, parens added for clarity */
		} /* end i */
		e[l] = 0.0;
		e[k] = f;
		q[k] = x;
		goto test_f_splitting;
convergence:
		if (z < 0.0) {
			/* q[k] is made non-negative */
			q[k] = -z;
			if (withv) {
				for (j = 0; j < n; j++) {
					v[j * n + k] = -v[j * n + k];
				}
			} /* end withv, parens added for clarity */
		} /* end z */
	} /* end k */

	free(e);
	return retval;
}


int svd_algol(int m, int n, int withu, int withv, double eps, double tol, double *a, double *q, double *u, double *v) {
	int i, j, k, l, l1, iter, retval;
	double c, f, g, h, s, x, y, z;
	double *e;

	e = (double *) calloc(n, sizeof (double));
	retval = 0;

	/* Copy 'a' to 'u' */
	for (i = 0; i < m; i++) {
		for (j = 0; j < n; j++) {
			u[i * m + j] = a[i * n + j];
		}
	}
	/* Householder's reduction to bidiagonal form. */
	g = x = 0.0;
	for (i = 0; i < n; i++) {
		e[i] = g;
		s = 0.0;
		l = i + 1;
		for (j = i; j < m; j++) {
			s += (u[j * m + i] * u[j * m + i]);
		}
		if (s < tol) {
			g = 0.0;
		} else {
			f = u[i * m + i];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			u[i * m + i] = f - g;
			for (j = l; j < n; j++) {
				s = 0.0;
				for (k = i; k < m; k++) {
					s += (u[k * m + i] * u[k * m + j]);
				}
				f = s / h;
				for (k = i; k < m; k++) {				
					u[k * m + j] += (f * u[k * m + i]);
				}
			} /* end j */
		} /* end s */
		q[i] = g;
		s = 0.0;
		for (j = l; j < n; j++) {
			s += (u[i * m + j] * u[i * m + j]);
		}
		if (s < tol) {
			g = 0.0;
		} else {
			f = u[i * m + (i + 1) ];
			g = (f < 0) ? sqrt(s) : -sqrt(s);
			h = f * g - s;
			u[i * m + (i + 1) ] = f - g;
			for (j = l; j < n; j++) {
				e[j] = u[i * m + j] / h;
			}
			for (j = l; j < m; j++) {
				s = 0.0;
				for (k = l; k < n; k++) {
					s += (u[j * m + k] * u[i * m + k]);
				}
				for (k = l; k < n; k++) {
					u[j * m + k] += (s * e[k]);
				}
			} /* end j */
		} /* end s */
		y = fabs(q[i]) + fabs(e[i]);
		if (y > x)
			x = y;
	} /* end i */
	

	/* accumulation of right-hand transformations */
	if (withv) {
		for (i = n - 1; i >= 0; i--) {
			if (g != 0.0) {
				h = u[i * m + (i + 1) ] * g;
				for (j = l; j < n; j++) {
					v[j * n + i] = u[i * m + j] / h;
				}
				for (j = l; j < n; j++) {
					s = 0.0;
					for (k = l; k < n; k++) {
						s += (u[i * m + k] * v[k * n + j]);
					}
					for (k = l; k < n; k++) {
						v[k * n + j] += (s * v[k * n + i]);
					}

				} /* end j */
			} /* end g */
			for (j = l; j < n; j++) {
				v[i * n + j] = v[j * n + i] = 0.0;
			}
			v[i * n + i] = 1.0;
			g = e[i];
			l = i;
		} /* end i */

	} /* end withv, parens added for clarity */

	/* accumulation of left-hand transformations */

	if (withu) {
	//if (0) {
		for (i = n; i < m; i++) {
			for (j = n; j < m; j++) {
				u[i * m + j] = 0.0;
			}
			u[i * m + i] = 1.0;
		}
	}
	if (withu) {
	//if (0) {
		for (i = n - 1; i >= 0; i--) {
			l = i + 1;
			g = q[i];
			for (j = l; j < m; j++) { /* upper limit was 'n' */
				u[i * m + j] = 0.0;
			}
			if (g != 0.0) {
				h = u[i * m + i] * g;
				for (j = l; j < m; j++) { /* upper limit was 'n' */
					s = 0.0;
					for (k = l; k < m; k++) {
						s += (u[k * m + i] * u[k * m + j]);
					}
					f = s / h;
					for (k = i; k < m; k++) {
						u[k * m + j] += (f * u[k * m + i]);
					}
				} /* end j */
				for (j = i; j < m; j++) {
					u[j * m + i] /= g;
				}
			}/* end g */
			else {
				for (j = i; j < m; j++) {
					u[j * m + i] = 0.0;
				}
			}
			u[i * m + i] += 1.0;
		} /* end i*/
	} /* end withu, parens added for clarity */

	/* diagonalization of the bidiagonal form */
	eps *= x;
	for (k = n - 1; k >= 0; k--) {
		iter = 0;
test_f_splitting:
		for (l = k; l >= 0; l--) {
			if (fabs(e[l]) <= eps) goto test_f_convergence;
			if (fabs(q[l - 1]) <= eps) goto cancellation;
		} /* end l */

		/* cancellation of e[l] if l > 0 */
cancellation:
		c = 0.0;
		s = 1.0;
		l1 = l - 1;
		for (i = l; i <= k; i++) {
			f = s * e[i];
			e[i] *= c;
			if (fabs(f) <= eps) {
				goto test_f_convergence;
			}
			g = q[i];
			h = q[i] = sqrt(f * f + g * g);
			c = g / h;
			s = -f / h;
			if (withu) {
				//if (0) {
				for (j = 0; j < m; j++) {
					y = u[j * m + l1];
					z = u[j * m + i];
					u[j * m + l1] = y * c + z * s;
					u[j * m + i] = -y * s + z * c;
				} /* end j */
			} /* end withu, parens added for clarity */
		} /* end i */
test_f_convergence:
		z = q[k];
		if (l == k) {
			goto convergence;
		}

		/* shift from bottom 2x2 minor */
		iter++;
		if (iter > 30) {
			retval = k;
			break;
		}
		x = q[l];
		y = q[k - 1];
		g = e[k - 1];
		h = e[k];
		f = ((y - z)*(y + z) + (g - h)*(g + h)) / (2 * h * y);
		g = sqrt(f * f + 1.0);
		f = ((x - z)*(x + z) + h * (y / ((f < 0) ? (f - g) : (f + g)) - h)) / x;
		/* next QR transformation */
		c = s = 1.0;
		for (i = l + 1; i <= k; i++) {
			g = e[i];
			y = q[i];
			h = s * g;
			g *= c;
			e[i - 1] = z = sqrt(f * f + h * h);
			c = f / z;
			s = h / z;
			f = x * c + g * s;
			g = -x * s + g * c;
			h = y * s;
			y *= c;
			if (withv) {
				for (j = 0; j < n; j++) {
					x = v[j * n + (i - 1)];
					z = v[j * n + i];
					v[j * n + (i - 1)] = x * c + z * s;
					v[j * n + i] = -x * s + z * c;
				} /* end j */
			} /* end withv, parens added for clarity */
			q[i - 1] = z = sqrt(f * f + h * h);
			c = f / z;
			s = h / z;
			f = c * g + s * y;
			x = -s * g + c * y;
			if (withu) {
				//if (0) {
				for (j = 0; j < m; j++) {
					y = u[j * m + (i - 1)];
					z = u[j * m + i];
					u[j * m + (i - 1)] = y * c + z * s;
					u[j * m + i] = -y * s + z * c;
				} /* end j */
			} /* end withu, parens added for clarity */
		} /* end i */
		e[l] = 0.0;
		e[k] = f;
		q[k] = x;
		goto test_f_splitting;
convergence:
		if (z < 0.0) {
			/* q[k] is made non-negative */
			q[k] = -z;
			if (withv) {
				for (j = 0; j < n; j++) {
					v[j * n + k] = -v[j * n + k];
				}
			} /* end withv, parens added for clarity */
		} /* end z */
	} /* end k */

	free(e);
	return retval;
}

/**
 * 
 * Decomposes a matrix A = U * S * V'
 * 
 * @param m - number of rows
 * @param n - number of columns
 * @param a - m by n input matrix
 * @param q - n by 1 matrix
 * @param u - m by m matrix
 * @param v - n by n matrix
 * @return 
 */
int svd(int m, int n, double * A, double * Q, double * U, double * V) {
	double eps = __eps;
	double tol = __tol;

	int withu = 1;
	int withv = 1;

	return svd_algol(m, n, withu, withv, eps, tol, A, Q, U, V);
}
